#include <iostream>
#include <Eigen/Dense>

int main (int argc, char** argv)
{   
    double roll, pitch, yaw;
    roll = 30;
    pitch = 45;
    yaw = 60;

    Eigen::Vector3d R_euler(roll, pitch, yaw); // 旋转
	R_euler = R_euler / 180 * M_PI;  // 角度转弧度
	Eigen::Matrix3d R;

    // 注意给的欧拉角顺序和大小
	R = Eigen::AngleAxisd(R_euler[2], Eigen::Vector3d::UnitZ()) *
	Eigen::AngleAxisd(R_euler[1], Eigen::Vector3d::UnitY()) *
	Eigen::AngleAxisd(R_euler[0], Eigen::Vector3d::UnitX());
	
    std::cout << R << std::endl;

    // ypr
    Eigen::Vector3d eu(R.eulerAngles(2, 1, 0));
    std::cout << "euler(ypr): \n"<< eu.transpose() << std::endl;

    Eigen::Vector3d eu2(R.eulerAngles(0, 1, 2));
    std::cout << "euler(rpy): \n" << eu2.transpose() << std::endl;

    Eigen::Quaterniond q(R);
    std::cout << "q(xyzw):\n"<< q.coeffs().transpose() << std::endl;

    return 0;
}